/*=====================================================================*/
/*  awesomescan                                                      */ 
/*  based on www.sjbaker.org/wiki/index.php?title=A_Simple_3D_Scanner  */
/*=====================================================================*/


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>
#include <string.h>

extern "C"
{
#include <jpeglib.h>
#include <jerror.h>
}

#include <math.h>

#define RADIANS_TO_DEGREES  (180.0f / 3.14159f )
#define DEGREES_TO_RADIANS  (3.14159f / 180.0f )
/*================================================================*/

#define ARG_HFOV 	"hfov"
#define ARG_VFOV 	"vfov"
#define ARG_DISTANCE 	"dist"
#define ARG_DEGREES 	"degrees"
#define ARG_H_AVG 	"h-average"
#define ARG_V_AVG 	"v-average"
#define ARG_FRAME_SKIP 	"skip-frame"
#define ARG_POINT_SKIP 	"skip-point"
#define ARG_CENTER 	"cor"
#define ARG_TOP 	"top"
#define ARG_BOT 	"bottom"
#define ARG_LEFT 	"left"
#define ARG_RIGHT 	"right"
#define ARG_HELP	"help"
#define ARG_VERSION	"v"
#define ARG_VERBOSE	"verbose"

#define VERSION_CODE 	"1.0"
/*================================================================*/
/*    USER EDITABLE SECTION:  Change these to suit your scanner   */
/*================================================================*/
float CAMERA_HFOV =50.0f;   /* Degrees */
float CAMERA_VFOV =(CAMERA_HFOV*4.0f/5.0f); /* Degrees */
float CAMERA_DISTANCE=0.30f;   /* Meters  */
float LASER_OFFSET=45.0f;   /* Degrees */
int HORIZ_AVG=2; /* Num horizontal points to average */
int VERT_AVG=2; /* Num vertical points to average */
int FRAME_SKIP=1;  /* Use every n'th frame for speed */
int POINT_SKIP=1;  /* Use every n'th scanline for speed*/

int IMAGE_CENTER = 0;
int IMAGE_TOP = 70;
int IMAGE_BOTTOM = 345;
int IMAGE_LEFT = 220;
int IMAGE_RIGHT = 380;
/*================================================================*/

class Image
{
protected:
  int width ;
  int height ;
  unsigned char *buffer ;

public:

  Image ( int w, int h )
  {
    width = w ;
    height = h ;
    buffer = new unsigned char [ w * h * 3 ] ;
  }


  Image ()
  {
    width = height = 0 ;
    buffer = NULL ;
  }


  virtual ~Image () ;

  int getWidth  () { return width  ; }
  int getHeight () { return height ; }

  unsigned char *getPixels () { return buffer ; }

  unsigned int getPixelArea ( float x1, float y1,
                              float x2, float y2,
                              unsigned int keyColour ) ;

  unsigned char getPixelRed ( int x, int y )
  {
    return (unsigned int) buffer [ ( y * width + x ) * 3 + 0 ] ;
  }

  unsigned char getPixelGreen ( int x, int y )
  {
    return (unsigned int) buffer [ ( y * width + x ) * 3 + 1 ] ;
  }

  unsigned char getPixelBlue ( int x, int y )
  {
    return (unsigned int) buffer [ ( y * width + x ) * 3 + 2 ] ;
  }

  unsigned int getPixel ( float x, float y )
  {
    return getPixel ( (int) x, (int) y ) ;
  }

  unsigned int getPixel ( int x, int y )
  {
    int p = ( y * width + x ) * 3 ;

    return ( (unsigned int) buffer [ p + 0 ] << 24 ) +
           ( (unsigned int) buffer [ p + 1 ] << 16 ) +
           ( (unsigned int) buffer [ p + 2 ] <<  8 ) + 255 ;
  }

  void setPixel ( float x, float y, unsigned int rgba )
  {
    setPixel ( (int) x, (int) y, rgba ) ;
  }

  void setPixel ( int x, int y, unsigned int rgba )
  {
    int p = ( y * width + x ) * 3 ;

    if ( ( rgba & 0xFF ) == 0 )
      return ;

    if ( ( rgba & 0xFF ) == 255 )
    {
      buffer [ p + 0 ] = ( rgba >> 24 ) & 0xFF ;
      buffer [ p + 1 ] = ( rgba >> 16 ) & 0xFF ;
      buffer [ p + 2 ] = ( rgba >>  8 ) & 0xFF ;
    }
    else
    {
      unsigned int r = ( rgba >> 24 ) & 0xFF ;
      unsigned int g = ( rgba >> 16 ) & 0xFF ;
      unsigned int b = ( rgba >>  8 ) & 0xFF ;
      unsigned int a = ( rgba >>  0 ) & 0xFF ;
      unsigned int ac = 255 - a ;

      buffer [ p + 0 ] = (unsigned char)((int)(buffer [ p + 0 ]) * ac / 255 + r * a / 255 ) ;
      buffer [ p + 1 ] = (unsigned char)((int)(buffer [ p + 1 ]) * ac / 255 + g * a / 255 ) ;
      buffer [ p + 2 ] = (unsigned char)((int)(buffer [ p + 2 ]) * ac / 255 + b * a / 255 ) ;
    }
  }

  virtual int load ( char *fname ) = 0 ;

} ;



class JPEG : public Image
{
public:

  JPEG () : Image () {}
  JPEG ( int w, int h ) : Image ( w, h ) {}

  virtual int load ( char *fname ) ;
} ;

static const int _endianTest = 1;
#define isLittleEndian (*((char *) &_endianTest ) != 0)
#define isBigEndian    (*((char *) &_endianTest ) == 0)

Image::~Image ()
{
  delete buffer ;
}


int JPEG::load ( char * filename )
{
  jpeg_decompress_struct cinfo        ;
  jpeg_error_mgr         jerr         ;

  FILE       *fd         ;
  JSAMPARRAY  linebuffer ;
  int         row_stride ;

  if ( ( fd  = fopen ( filename, "rb" ) ) == NULL )
  {
    perror  ( "readJPEG" ) ;
    fprintf ( stderr, "readJPEG: Can't open %s for reading\n", filename ) ;
    return 0 ;
  }


  cinfo.err = jpeg_std_error ( &jerr  ) ;
  jpeg_create_decompress     ( &cinfo ) ;
  jpeg_stdio_src             ( &cinfo, fd ) ;
  jpeg_read_header           ( &cinfo, TRUE ) ;
  jpeg_start_decompress      ( &cinfo ) ;

  row_stride = cinfo.output_width * cinfo.output_components ;

  if ( cinfo.output_components != 3 )
  {
    fprintf ( stderr, "readJPEG: %s has %d components?!?\n", filename,
                                       cinfo.output_components ) ;
    return 0 ;
  }

  linebuffer = (*cinfo.mem->alloc_sarray)
		      ( (j_common_ptr) &cinfo, JPOOL_IMAGE, row_stride, 1 ) ;

  delete buffer ;

  height = cinfo.output_height ;
  width  = cinfo.output_width  ;

  buffer = new unsigned char [ width * height * 3 ] ;

  while (cinfo.output_scanline < cinfo.output_height)
  {
    int y = cinfo.output_scanline ;

    /*
      jpeg_read_scanlines expects an array of pointers to scanlines.
      Here the array is only one element long, but you could ask for
      more than one scanline at a time if that's more convenient.
    */

    jpeg_read_scanlines ( &cinfo, linebuffer, 1 ) ;
    memcpy ( & buffer [ y * row_stride ], linebuffer[0], row_stride ) ;
  }

  jpeg_finish_decompress  ( &cinfo ) ;
  jpeg_destroy_decompress ( &cinfo ) ;

  fclose ( fd ) ;
  return 1 ;
}


unsigned int Image::getPixelArea ( float x1, float y1,
                                  float x2, float y2,
                                  unsigned int keyColour )
{
  if ( x2-x1 <= 0.0f || y2-y1 <= 0.0f )
    return 0x00000000 ;

  unsigned int r = ( keyColour >> 24 ) & 0xFF ;
  unsigned int g = ( keyColour >> 16 ) & 0xFF ;
  unsigned int b = ( keyColour >>  8 ) & 0xFF ;

  float area_tot = (x2-x1) * (y2-y1) ;

  float r_tot = 0.0f ;
  float g_tot = 0.0f ;
  float b_tot = 0.0f ;

  float rgb_area = 0.0f ;

  for ( int i = (int)floor(x1) ; i <= (int)ceil(x2) ; i++ )
    for ( int j = (int)floor(y1) ; j <= (int)ceil(y2) ; j++ )
    {
      if ( i < 0 || i >= width  ||
           j < 0 || j >= height ||
           getPixel( i, j ) == keyColour )
        continue ;

      float xa = ( x1 > (float)  i  ) ? x1 : (float)   i   ;
      float xb = ( x2 < (float)(i+1)) ? x2 : (float) (i+1) ;
      float ya = ( y1 > (float)  j  ) ? y1 : (float)   j   ;
      float yb = ( y2 < (float)(j+1)) ? y2 : (float) (j+1) ;

      if ( xb-xa <= 0.0f || yb-ya <= 0.0f )
        continue ;

      float area = (xb-xa) * (yb-ya) ;

      rgb_area += area ;

      r_tot += getPixelRed   ( i, j ) * area ;
      g_tot += getPixelGreen ( i, j ) * area ;
      b_tot += getPixelBlue  ( i, j ) * area ;
    }

  if ( rgb_area <= 0.0f )
    return 0x00000000 ;

  r_tot /= rgb_area ;
  g_tot /= rgb_area ;
  b_tot /= rgb_area ;

  float a_tot = rgb_area * 255.0f / area_tot ;

  if ( r_tot > 255.0f ) r_tot = 255.0f ;
  if ( g_tot > 255.0f ) g_tot = 255.0f ;
  if ( b_tot > 255.0f ) b_tot = 255.0f ;
  if ( a_tot > 255.0f ) a_tot = 255.0f ;

  if ( r_tot <= 0.0f ) r_tot = 0.0f ;
  if ( g_tot <= 0.0f ) g_tot = 0.0f ;
  if ( b_tot <= 0.0f ) b_tot = 0.0f ;
  if ( a_tot <= 0.0f ) a_tot = 0.0f ;

  return ( (unsigned int) r_tot << 24 ) +
         ( (unsigned int) g_tot << 16 ) +
         ( (unsigned int) b_tot << 8  ) +
         ( (unsigned int) a_tot << 0  ) ;
}


void ASAtoSAS ( float  angA, float  lenB, float  angC,
                float *lenA, float *angB, float *lenC )
{
  /* Find the missing angle */

  float bb = 180.0f - (angA + angC) ;

  if ( angB ) *angB = bb ;

  /* Convert everything to radians */

  angA *= DEGREES_TO_RADIANS ;
  angC *= DEGREES_TO_RADIANS ;
  bb   *= DEGREES_TO_RADIANS ;

  /* Use Sine Rule */

  float sinB = sin ( bb ) ;

  if ( sinB == 0.0f )
  {
    if ( lenA ) *lenA = lenB / 2.0f ;  /* One valid interpretation */
    if ( lenC ) *lenC = lenB / 2.0f ;
  }
  else
  {
    if ( lenA ) *lenA = lenB * sin(angA) / sinB ;
    if ( lenC ) *lenC = lenB * sin(angC) / sinB ;
  }
}


float *processRawFrame ( char *fname,char *cfname, int f, int num_frames, int *num_points )
{
  JPEG *jpg = new JPEG ;
  JPEG *jpgcolor = new JPEG ;
  float R,G,B ;
   
  jpg -> load ( fname ) ;
  jpgcolor -> load ( cfname ) ;
  int np = jpg->getHeight() / POINT_SKIP ;
  //int np = (IMAGE_BOTTOM - IMAGE_TOP ) / POINT_SKIP;
  *num_points = ( IMAGE_BOTTOM - IMAGE_TOP ) / POINT_SKIP; 
  
  float *res = new float [ 6 * np ] ;

  float frame_angle = ((float) f) * (360.0f / (float) num_frames) ;

  for ( int j = 0 ; j < np ; j++ )
  {
  	if (j > IMAGE_TOP && j < IMAGE_BOTTOM )
  	{
  	
  	int jj = j - IMAGE_TOP;
  	
    /* Find the brightest pixel */
		R = 0.0f;
		G = 0.0f;
		B = 0.0f;
    float max  = 0.0f ;
    int maxpos = -1 ;
    int maxfromcenter = -1;
    for ( int i = 0 ; i < jpg -> getWidth () ; i++ )
    {
	if (i > IMAGE_LEFT && i < IMAGE_RIGHT )
	{
    
      		unsigned int px = jpg -> getPixel ( i, j*POINT_SKIP ) ;

      		float brightness = ((float)(( px >> 24 ) & 0xFF)) / 255.0f +
                         ((float)(( px >> 16 ) & 0xFF)) / 255.0f +
                         ((float)(( px >>  8 ) & 0xFF)) / 255.0f ;

      		if ( brightness > max )
      		{
       			max = brightness ;
        		maxpos = i ;
			maxfromcenter =  IMAGE_CENTER - i;
		//rgb
			unsigned int px = jpgcolor -> getPixel ( i, j*POINT_SKIP) ;
			R = ((float)(( px >> 24 ) & 0xFF))/4;
			G = ((float)(( px >> 16 ) & 0xFF))/4;
			B = ((float)(( px >> 8 ) & 0xFF))/4;
	 	}
	 }
	 
    }

    float radius ;
    float camera_angle;
    float x,y,z;
    z = 0.0f;
    
    if (! IMAGE_CENTER ) {
    	camera_angle = CAMERA_HFOV *
                         (0.5f - (float)maxpos / 
                         (float)jpg -> getWidth ()) ;
                         
    	ASAtoSAS ( camera_angle, CAMERA_DISTANCE, LASER_OFFSET,
               & radius, NULL, NULL ) ;
    }
    else {
    	camera_angle = CAMERA_HFOV *
    			((float) maxfromcenter / //(float) jpg ->getWidth()
    			(float) ((IMAGE_BOTTOM - IMAGE_TOP) / POINT_SKIP ) // this is wrong!!
    			);
    			
    	ASAtoSAS ( camera_angle, CAMERA_DISTANCE, LASER_OFFSET,
               & radius, NULL, NULL ) ;
    	//radius = (float) maxfromcenter / CAMERA_VFOV;
    	/*
    	z = atan ( (CAMERA_VFOV * DEGREES_TO_RADIANS / 2.0f) ) *
                  2.0f * CAMERA_DISTANCE * (float) jj / (float) 
                  ( (IMAGE_BOTTOM - IMAGE_TOP )/ POINT_SKIP) ;//
                  //np;
        */
        //z = (float) jj;
    	fprintf(stderr, "center of rotation method\r");
    }

    //ASAtoSAS ( camera_angle, CAMERA_DISTANCE, LASER_OFFSET,
    //           & radius, NULL, NULL ) ;

    x = radius * sin ( frame_angle * DEGREES_TO_RADIANS ) ;
    y = radius * cos ( frame_angle * DEGREES_TO_RADIANS ) ;
    if (z == 0) {
    	z = atan ( (CAMERA_VFOV * DEGREES_TO_RADIANS / 2.0f) ) *
                  2.0f * CAMERA_DISTANCE * (float) jj / (float) 
                  ( (IMAGE_BOTTOM - IMAGE_TOP )/ POINT_SKIP) ;//np;
    }
    // if ( max < 1.50 )
    //   x = y = 0.0f ;

    res [ 6 * jj + 0 ] = x ;
    res [ 6 * jj + 1 ] = y ;
    res [ 6 * jj + 2 ] = z ;
    res [ 6 * jj + 3 ] = R ;
	res [ 6 * jj + 4 ] = G ;
	res [ 6 * jj + 5 ] = B ;
	}
   }
  delete jpg ;
  delete jpgcolor ;
  return res ;
}


void outputFrames ( int num_points, int num_frames, float **vertices )
{
  int num_outframes = num_frames / HORIZ_AVG ;
  int num_outpoints = num_points / VERT_AVG ;

  long prune_num = 0;

  
  for ( long f = 0 ; f < num_outframes ; f++ )
    for ( long i = 0 ; i < num_outpoints ; i++ )
    {
      float avgt [ 6 ] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f } ;

      for ( long ff = 0 ; ff < HORIZ_AVG ; ff++ )
      {
        for ( long ii = 0 ; ii < VERT_AVG ; ii++ )
        {
          
          avgt [ 0 ] += vertices[f*HORIZ_AVG+ff][(i*VERT_AVG+ii)*6+0] ;
          avgt [ 1 ] += vertices[f*HORIZ_AVG+ff][(i*VERT_AVG+ii)*6+1] ;
          avgt [ 2 ] += vertices[f*HORIZ_AVG+ff][(i*VERT_AVG+ii)*6+2] ;
	avgt [ 3 ] += vertices[f*HORIZ_AVG+ff][(i*VERT_AVG+ii)*6+3] ;
	avgt [ 4 ] += vertices[f*HORIZ_AVG+ff][(i*VERT_AVG+ii)*6+4] ;
	avgt [ 5 ] += vertices[f*HORIZ_AVG+ff][(i*VERT_AVG+ii)*6+5] ;
			
		}
      }

	  avgt [ 0 ] /= (float)( HORIZ_AVG*VERT_AVG ) ;
      avgt [ 1 ] /= (float)( HORIZ_AVG*VERT_AVG ) ;
      avgt [ 2 ] /= (float)( HORIZ_AVG*VERT_AVG ) ;

      if (avgt[0] + avgt[1] + avgt[2] != 0 ) {
      	prune_num ++ ;
      }
    }
  if (prune_num > num_outpoints * num_frames) prune_num = num_outpoints * num_frames;
   
   
  printf("ply\n");
  printf("format ascii 1.0\n");
  printf("comment Made with awesomescan!\n");
   
  printf("element vertex %d\n", prune_num);// num_outpoints*num_outframes);
  
  printf("property float x\n");
  printf("property float y\n");
  printf("property float z\n");
  printf("property float nx\n");
  printf("property float ny\n");
  printf("property float nz\n");
  printf("property uchar red\n");
  printf("property uchar green\n");
  printf("property uchar blue\n");
  printf("end_header\n");
  
  for ( long f = 0 ; f < num_outframes ; f++ )
    for ( long i = 0 ; i < num_outpoints ; i++ )
    {
      float avg [ 6 ] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f } ;

      for ( long ff = 0 ; ff < HORIZ_AVG ; ff++ )
      {
        for ( long ii = 0 ; ii < VERT_AVG ; ii++ )
        {
          
          avg [ 0 ] += vertices[f*HORIZ_AVG+ff][(i*VERT_AVG+ii)*6+0] ;
          avg [ 1 ] += vertices[f*HORIZ_AVG+ff][(i*VERT_AVG+ii)*6+1] ;
          avg [ 2 ] += vertices[f*HORIZ_AVG+ff][(i*VERT_AVG+ii)*6+2] ;
	avg [ 3 ] += vertices[f*HORIZ_AVG+ff][(i*VERT_AVG+ii)*6+3] ;
	avg [ 4 ] += vertices[f*HORIZ_AVG+ff][(i*VERT_AVG+ii)*6+4] ;
	avg [ 5 ] += vertices[f*HORIZ_AVG+ff][(i*VERT_AVG+ii)*6+5] ;
			
		}
      }

	  avg [ 0 ] /= (float)( HORIZ_AVG*VERT_AVG ) ;
      avg [ 1 ] /= (float)( HORIZ_AVG*VERT_AVG ) ;
      avg [ 2 ] /= (float)( HORIZ_AVG*VERT_AVG ) ;

      if (avg[0] + avg[1] + avg[2] != 0 ) {
      	printf ( "%f %f %f %f %f 0.000000 %i %i %i\n", avg [ 0 ], avg [ 1 ], avg [ 2 ], avg [ 0 ], avg [ 1 ], int(avg [ 3 ] ), int(avg [ 4 ] ), int(avg [ 5 ] )) ;
      }
    }

}

void printHelp() {
	fprintf(stderr, "HELP TEXT:\n");
	
	fprintf(stderr, "usage: awesomescan [options] > scan.ply\n\n" );
	fprintf(stderr, "\t-%s \tHelp text\n", ARG_HELP );
	fprintf(stderr, "\t-%s <pixels> \t\tImage top (bounding box)\n", ARG_TOP  );
	fprintf(stderr, "\t-%s <pixels> \tImage bottom (bounding box)\n", ARG_BOT  );
	fprintf(stderr, "\t-%s <pixels> \t\tImage left (bounding box)\n", ARG_LEFT  );
	fprintf(stderr, "\t-%s <pixels> \tImage right (bounding box)\n", ARG_RIGHT  );
	
	fprintf(stderr, "\t-%s <pixels> \t\tImage center of rotation\n", ARG_CENTER);
	
	fprintf(stderr, "\t-%s <degrees> \tCam HFOV\n", ARG_HFOV  );
	fprintf(stderr, "\t-%s <degrees> \tCam VFOV\n", ARG_VFOV  );
	fprintf(stderr, "\t-%s <meters> \t\tImage distance\n", ARG_DISTANCE  );
	fprintf(stderr, "\t-%s <degrees> \tCam angle\n", ARG_DEGREES  );
	
	fprintf(stderr, "\t-%s <points> \tNum of H pts to average\n", ARG_H_AVG  );
	fprintf(stderr, "\t-%s <points> \tNum of V pts to average\n", ARG_V_AVG  );
	
	fprintf(stderr, "\t-%s <frames> \tNum of frames to skip (speed)\n", ARG_FRAME_SKIP  );
	fprintf(stderr, "\t-%s <pixels> \tNum of points to skip (speed)\n", ARG_POINT_SKIP  );
	
	fprintf(stderr, "\t-%s  \t\t\tVersion\n", ARG_VERSION  );
	
	fprintf(stderr, "\t-%s  \t\tVerbose printout\n", ARG_VERBOSE  );
	
	fprintf(stderr, "\n");
}

void arguments (int argc, char **argv) {
	int version = FALSE;
	int help = FALSE;
	int verbose = FALSE;
	for (int i = 1; i < argc; i ++ )
	{
		
		if (strcmp( &argv[i][1], ARG_VERSION) == 0 ) version = TRUE;
		if (strcmp( &argv[i][1], ARG_HELP) == 0 ) help = TRUE;
		if (strcmp( &argv[i][1], ARG_TOP) == 0 ) IMAGE_TOP = atoi( argv[i + 1]); 
		
		if (strcmp( &argv[i][1], ARG_BOT) == 0 ) IMAGE_BOTTOM = atoi(argv[i+1]);
		if (strcmp( &argv[i][1], ARG_LEFT) == 0 ) IMAGE_LEFT = atoi(argv[i+1]);
		if (strcmp( &argv[i][1], ARG_RIGHT) == 0 ) IMAGE_RIGHT = atoi(argv[i+1]);
		if (strcmp( &argv[i][1], ARG_CENTER) == 0 ) IMAGE_CENTER = atoi(argv[i+1]);
		
		if (strcmp( &argv[i][1], ARG_HFOV) == 0 ) CAMERA_HFOV = atof(argv[i+1]);
		if (strcmp( &argv[i][1], ARG_VFOV) == 0 ) CAMERA_VFOV = atof(argv[i+1]);
		
		if (strcmp( &argv[i][1], ARG_H_AVG) == 0 ) HORIZ_AVG = atoi(argv[i+1]);
		if (strcmp( &argv[i][1], ARG_V_AVG) == 0 ) VERT_AVG = atoi(argv[i+1]);
		
		if (strcmp( &argv[i][1], ARG_DISTANCE) == 0 ) CAMERA_DISTANCE = atof(argv[i+1]);
		if (strcmp( &argv[i][1], ARG_DEGREES) == 0 ) LASER_OFFSET = atof(argv[i+1]);
		
		if (strcmp( &argv[i][1], ARG_FRAME_SKIP) == 0 ) FRAME_SKIP = atoi(argv[i+1]);
		if (strcmp( &argv[i][1], ARG_POINT_SKIP) == 0 ) POINT_SKIP = atoi(argv[i+1]);
		if (strcmp( &argv[i][1], ARG_VERBOSE) == 0 ) verbose = TRUE;
	}
	
	if (help) {
		printHelp();
	}
	if (version) {
		fprintf(stderr, "version %s\n", VERSION_CODE);
	}
	
	if (verbose) {
	
		fprintf(stderr, " -%s %d\n", ARG_TOP, IMAGE_TOP);
		fprintf(stderr, " -%s %d\n", ARG_BOT, IMAGE_BOTTOM);
		fprintf(stderr, " -%s %d\n", ARG_LEFT, IMAGE_LEFT);
		fprintf(stderr, " -%s %d\n", ARG_RIGHT, IMAGE_RIGHT);
		fprintf(stderr, " -%s %d\n", ARG_CENTER, IMAGE_CENTER);
		
		fprintf(stderr, " -%s %d\n", ARG_H_AVG, HORIZ_AVG);
		fprintf(stderr, " -%s %d\n", ARG_V_AVG, VERT_AVG);
		
		fprintf(stderr, " -%s %d\n", ARG_FRAME_SKIP, FRAME_SKIP);
		fprintf(stderr, " -%s %d\n", ARG_POINT_SKIP, POINT_SKIP);
		
		fprintf(stderr, " -%s %g\n", ARG_HFOV, CAMERA_HFOV);
		fprintf(stderr, " -%s %g\n", ARG_VFOV, CAMERA_VFOV);
		fprintf(stderr, " -%s %g\n", ARG_DISTANCE, CAMERA_DISTANCE);
		fprintf(stderr, " -%s %g\n", ARG_DEGREES, LASER_OFFSET);
		fprintf(stderr, "\n");
	}
}


int main ( int argc, char **argv )
{
  float **vertices ;
  int num_frames = 0 ;
  int count;
   
  
  arguments(argc, argv);
  
  for ( int i = 0 ; true ; i++ )
  {
    FILE *tmp ;
    char fname [ 100 ] ;

    sprintf ( fname, "%08d.jpg", i*FRAME_SKIP ) ;

    fprintf ( stderr, "Checking %s\r", fname ) ;

    if ( (tmp = fopen ( fname, "r" )) == NULL )
      break ;

    fclose ( tmp ) ;

    num_frames = i+1 ;
  }

  fprintf ( stderr, "\nProcessing %d frames...\n", num_frames ) ;

  vertices = new float * [ num_frames ] ;

  int npoints = -1 ;

  for ( int i = 0 ; i < num_frames ; i++ )
  {
    int np ;
    char fname [ 100 ] ;
	char cfname [ 100 ] ;
    sprintf ( fname, "%08d.jpg", i*FRAME_SKIP ) ;
    sprintf ( cfname, "%08d.jpg", i*FRAME_SKIP ) ;

    fprintf ( stderr, "Processing frame %d/%d    '%s'\r",
                                          i, num_frames, fname ) ;

    vertices [ i ] = processRawFrame ( fname,cfname, i, num_frames, &np ) ;

    assert ( npoints == -1 || np == npoints ) ;

    npoints = np ;
  }
  
  fprintf ( stderr, "\nOutputting...\n", num_frames ) ;
  outputFrames ( npoints, num_frames, vertices ) ;
}

